/**
 * @author Dennis Meyer, Sebastian Brodehl
 *
 */
import java.util.ArrayList;
import java.util.Comparator;
import java.util.PriorityQueue;
import java.util.Stack;


public class AStar {
	public aStarNode[][] map;
	private int heuristic;
	
	private aStarNode start;
	public aStarNode goal;
	
	private PriorityQueue<aStarNode> openList;
	private Stack<aStarNode> path;
	
	
	/**
	 * @param map
	 * @param start
	 * @param goal
	 * @param heuristic (1 Manhattan, 2 Euclidean, 3 Chebyshev)
	 */
	public AStar(int[][] map, aStarNode start, aStarNode goal, int heuristic) {
		this.generateMap(map);
		this.heuristic = heuristic;
		this.start = start;
		this.goal = goal;
	
		this.openList = new PriorityQueue<aStarNode>(10, new Comparator<aStarNode>() {
			@Override
			public int compare(aStarNode node1, aStarNode node2) {
				double val = node1.f - node2.f;
				if( val < 0)
					return -1;
				if( val > 0)
					return 1;
				return 0;
			}
			
		});
		
		start = this.map[start.getY()][start.getX()];
		start.g = 0;
	    start.f = 0;
	    
	    openList.add(start);
	    start.visited = true;
	    
	    while (!openList.isEmpty()) {
	    	aStarNode node = openList.poll();
	        node.visited = true;
	        
	        if (node.getX() == goal.getX() && node.getY() == goal.getY()) {
	        	this.backTrace(node);
	        	return;
	        }
	        
	        ArrayList<aStarNode> neighbors = this.getNeighbors(node);
	        
	        for(aStarNode neighbor : neighbors) {
	        	int x = neighbor.getX();
	        	int y = neighbor.getY();
	        	
	        	double ng = node.g + ((x - node.getX() == 0 || y - node.getY() == 0) ? 1 : Math.sqrt(2.0));
	        		        	
	        	if (!neighbor.opened || ng < neighbor.g) {
	        		
	        		if(neighbor.visited)
	        			continue;
	        		
	        		neighbor.g = ng;
	        		if(neighbor.h == Double.POSITIVE_INFINITY)
	        			neighbor.h = 1.0 * this.heuristic(Math.abs(x - goal.getX()), Math.abs(y - goal.getY()));
	                neighbor.f = neighbor.g + neighbor.h;
	                neighbor.parent = node;
	                
	                if(!neighbor.opened) {
	                	openList.add(neighbor);
	                	neighbor.opened = true;
	                } else {
	                	openList.remove(neighbor);
	                	openList.add(neighbor);
	                }
	        	}
	        }
	    }
	}

	private void backTrace(aStarNode node) {
		Stack<aStarNode> path = new Stack<>();
		path.push(node);
		while (node.parent != null) {
			node = node.parent;
			path.push(node);
	    }
		this.path = path;
	}
	
	@SuppressWarnings("unchecked")
	public void printPath() {
		if(this.path.isEmpty()) {
			System.out.println("*** No Path from "+ this.start +"to " + this.goal + "available! ***");
			return;
		}
		System.out.println("*** Path from "+ this.start +"to " + this.goal + "***");
		Stack<aStarNode> tmpPath = (Stack<aStarNode>) this.path.clone();
		aStarNode node;
		while(!tmpPath.isEmpty()) {
			node = tmpPath.pop();
			System.out.println(node);
		}
		System.out.println("*** The End. ***");
	}	

	private double heuristic(int dx, int dy) {
		switch (heuristic) {
		//Manhattan distance
		case 1:
			return dx + dy;
		//Euclidean distance
		case 2:
			return Math.sqrt(dx * dx + dy * dy);
		//Chebyshev distance
		case 3:
			return Math.max(dx, dy);
		default:
			return dx + dy;
		}
	}

	private void generateMap(int[][] map) {
		int yVal = map.length;
		int xVal = map[0].length;
		
		this.map = new aStarNode[yVal][xVal];
		
		for(int y = 0; y < map.length; y++) {
			for(int x = 0; x < map[0].length; x++) {
				aStarNode node = new aStarNode(x, y);
				if(map[y][x] > 0)
					node.walkable = true;
				this.map[y][x] = node;
			}
		}
	}

	private ArrayList<aStarNode> getNeighbors(aStarNode node) {
		ArrayList<aStarNode> neighbors = new ArrayList<>();
		
		int x = node.getX();
		int y = node.getY();
		
		if( this.isWalkable(x,y+1) && !this.map[x][y+1].visited )
			neighbors.add(new aStarNode(x, y+1));

		if( this.isWalkable(x+1, y) && !this.map[x+1][y].visited )
			neighbors.add(new aStarNode(x+1, y));

		if( this.isWalkable(x, y-1) && !this.map[y-1][x].visited )
			neighbors.add(new aStarNode(x, y-1));

		if( this.isWalkable(x-1, y) && !this.map[y][x-1].visited )
			neighbors.add(new aStarNode(x-1, y));		

		return neighbors;
	}

	private boolean isWalkable(int x, int y) {
		if(y < this.map.length - 1 && y >= 0 
				&& x < this.map[0].length && x >= 0
					&& this.map[y][x].walkable)
			return true;
		return false;
	}
	
	
}
